#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
碰撞检测模块

该模块提供了用于检测机器人与环境之间碰撞的CollisionChecker类。
"""


class CollisionChecker:
    """
    用于检测机器人与环境之间碰撞的类。
    """
    
    def __init__(self, map, robot):
        """
        使用地图和机器人初始化碰撞检测器。
        
        参数:
            map: 表示环境的地图对象
            robot: 表示机器人的对象
        """
        self.map = map
        self.robot = robot
    
    def check_collision(self, x, y):
        """
        检查位于(x, y)位置的机器人是否与障碍物或边界发生碰撞。
        
        参数:
            x (float): 机器人的X坐标
            y (float): 机器人的Y坐标
            
        返回:
            bool: 如果发生碰撞返回True，如果无碰撞返回False
        """
        # 使用地图的碰撞检测功能检查碰撞
        return self.map.check_collision(x, y, self.robot.radius)
    
    def check_path_collision(self, path, resolution=0.1):
        """
        检查路径是否有任何碰撞。
        
        参数:
            path (list): 表示路径的(x, y)坐标列表
            resolution (float): 沿线段的碰撞检测分辨率
            
        返回:
            bool: 如果路径上有任何碰撞返回True，如果无碰撞返回False
        """
        if len(path) < 2:
            return False
            
        # 检查路径的每个线段
        for i in range(len(path) - 1):
            if self.check_segment_collision(path[i], path[i + 1], resolution):
                return True
                
        return False
    
    def check_segment_collision(self, from_point, to_point, resolution=0.1):
        """
        检查两点之间的线段是否有任何碰撞。
        
        参数:
            from_point (tuple): 起始点 (x, y)
            to_point (tuple): 终止点 (x, y)
            resolution (float): 沿线段的碰撞检测分辨率
            
        返回:
            bool: 如果发生碰撞返回True，如果无碰撞返回False
        """
        # 提取坐标
        from_x, from_y = from_point
        to_x, to_y = to_point
        
        # 计算线段长度和方向
        dx = to_x - from_x
        dy = to_y - from_y
        
        segment_length = ((dx)**2 + (dy)**2)**0.5
        
        # 要检查的中间点数
        num_points = max(2, int(segment_length / resolution))
        
        # 沿线段定期间隔检查碰撞
        for i in range(num_points):
            t = i / (num_points - 1)  # 插值参数 [0, 1]
            x = from_x + t * dx
            y = from_y + t * dy
            
            if self.check_collision(x, y):
                return True  # 检测到碰撞
                
        return False  # 无碰撞